package com.hitqz.robot.driver.netty.handler;

import cn.hutool.core.util.ByteUtil;
import cn.hutool.core.util.HexUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.google.common.util.concurrent.RateLimiter;
import com.hitqz.robot.api.business.enums.VoiceEnum;
import com.hitqz.robot.api.business.event.PlayVoiceEvent;
import com.hitqz.robot.api.common.dto.warning.AddWarningDto;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.dto.KcLocationDto;
import com.hitqz.robot.driver.dto.KcNavStatus;
import com.hitqz.robot.driver.dto.KcRobotRunningStatus;
import com.hitqz.robot.driver.dto.KcStopStatusDto;
import com.hitqz.robot.driver.netty.protocol.message.KcMessage;
import com.hitqz.robot.driver.util.*;
import io.netty.channel.ChannelHandler;
import io.netty.channel.ChannelHandlerContext;
import io.netty.channel.SimpleChannelInboundHandler;
import lombok.extern.slf4j.Slf4j;

import java.nio.ByteOrder;
import java.nio.charset.StandardCharsets;
import java.text.DecimalFormat;
import java.util.Arrays;
import java.util.Objects;
import java.util.Optional;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.AtomicLong;

/**
 * @author xupkun
 * @date 2023/8/4
 */
@Slf4j
@ChannelHandler.Sharable
public class KcMessageHandler extends SimpleChannelInboundHandler<KcMessage> {

    private final static RateLimiter publishStatusLimiter = RateLimiter.create(1);

    private final DecimalFormat df = new DecimalFormat("0.##");

    private final AtomicInteger navFailCount = new AtomicInteger(0);

    private final AtomicLong noBlockedTimestamp = new AtomicLong(System.currentTimeMillis());

    private double blockedPositionX;

    private double blockedPositionY;

    private boolean sendBlockedWarningFlag = false;

    @Override
    protected void channelRead0(ChannelHandlerContext channelHandlerContext, KcMessage message) throws Exception {
        // 处理执行码
        this.checkExecuteCode(message);
        // 处理命令应答报文
        int cmd = message.getCmd()&0xFF;
        byte[] data = message.getData();
        switch (cmd) {
            case 0x00:
                //写变量
                break;
            case 0x01:
                //读变量
                //x00 U8[16] 变量名，采用 ascii 编码
                //0x10 U8[256] 变量值
                byte[] nameBytes = Arrays.copyOfRange(data, 0x00, 0x10);
                String varName = new String(nameBytes, StandardCharsets.US_ASCII).trim();
                //变量值的大小取决于该变量的变量类型
                byte[] valueBytes = Arrays.copyOfRange(data, 0x10, 0x10 + 256);
                DataTypeEnum dataTypeEnum = DataTypeEnum.VAR_NAME_ENUM_MAP.get(varName);
                String value = "";
                if (dataTypeEnum == null) {
                    log.warn("[kc]变量：{},not dataTypeEnum",varName);
                }else if (dataTypeEnum==DataTypeEnum.UDINT){
                    value = String.valueOf(ByteUtil.bytesToInt(valueBytes, ByteOrder.LITTLE_ENDIAN));
                }else if (dataTypeEnum==DataTypeEnum.REAL){
                    value = String.valueOf(ByteUtil.bytesToFloat(valueBytes, ByteOrder.LITTLE_ENDIAN));
                }else if (dataTypeEnum==DataTypeEnum.INT){
                    value = String.valueOf(ByteUtil.bytesToShort(valueBytes, ByteOrder.LITTLE_ENDIAN));
                }else if (dataTypeEnum==DataTypeEnum.BOOL){
                    value = String.valueOf(valueBytes[0]);
                }
                //log.info("varName: {} --- value {}", varName, value);
                if (Objects.equals(varName,"rep_BMS_Protect") && StrUtil.isNotBlank(value)){
                    int i = Integer.parseInt(value) & 0xFFFF;
                    String str = "正常";
                    if (i != 0){
                        str = "错误码:(hex)" + HexUtil.toHex(i);
                        // 报警
                        AddWarningDto addWarningDto = new AddWarningDto();
                        addWarningDto.setCode("");
                        addWarningDto.setTitle("BMS报警");
                        addWarningDto.setContent(str);
                        addWarningDto.setSuggestion("检测电池状态");
                        addWarningDto.setLevel(2);
                        SpringUtil.getApplicationContext().publishEvent(addWarningDto);
                    }
                    KcCacheUtil.setVarCache("rep_BMS_Protect_str",str);
                }
                KcCacheUtil.setVarCache(varName,value);
                break;
            case 0x11:
                 //手自动切换
                 break;
            case 0x14:
                 //机器人手动定位
                 break;
            case 0x16:
                 //导航控制
                 break;
            case 0x17: {
                //查询机器人运行状态
                KcRobotRunningStatus lastRunningStatus = Optional.ofNullable((KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS)).orElse(new KcRobotRunningStatus());
                double bodyTemperature = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x00, 0x08), ByteOrder.LITTLE_ENDIAN);//本体温度
                double positionX = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x08, 0x10), ByteOrder.LITTLE_ENDIAN);//x坐标
                double positionY = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x10, 0x18), ByteOrder.LITTLE_ENDIAN);//y坐标
                double orientationAngle = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x18, 0x20), ByteOrder.LITTLE_ENDIAN);//转向
                double batteryPower = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x20, 0x28), ByteOrder.LITTLE_ENDIAN);//电量 0~1
                int isBlocked = data[0x28] & 0xff;//是否被阻挡 0-否 其他-是
                int isCharging = data[0x29] & 0xff;//是否在充电 0-否 1-是
                if (isBlocked==0) {
                    //未阻挡，更新时间戳
                    noBlockedTimestamp.set(System.currentTimeMillis());
                    sendBlockedWarningFlag = true;
                }else if (lastRunningStatus.getIsBlocked()!=isBlocked){
                    //阻挡，且状态刚变化，记录坐标
                    log.info("[kc]robot is blocked...");
                    blockedPositionX = positionX;
                    blockedPositionY = positionY;
                }else if (isCharging == 0 && System.currentTimeMillis()-noBlockedTimestamp.get()>30000){
                    //阻挡，时间超过30s且位置变化小，发送告警
                    double distance =
                            Math.sqrt(Math.pow(blockedPositionX - positionX, 2) + Math.pow(blockedPositionY - positionY, 2));
                    if (distance < 0.01 && sendBlockedWarningFlag) {

                        PositionUtil positionUtil = SpringUtil.getBean("positionUtil");
                        boolean isNearPoint = positionUtil.checkPosition(positionX, positionY);
                        if (!isNearPoint) {
                            KcToolkit.publishWarningEvent("避障告警", "激光雷达触发，且30秒内位置未发生变化", "", "查看机器人周围是否有障碍物", 2);
                            sendBlockedWarningFlag = false;
                            PlayVoiceEvent playVoiceEvent = new PlayVoiceEvent();
                            playVoiceEvent.setVoiceEnum(VoiceEnum.LIDAR_STOP);
                            SpringUtil.getApplicationContext().publishEvent(playVoiceEvent);
                        }
                    }
                }
                int runMode = data[0x2A] & 0xff;//运行模式 0-手动 1-自动
                int mapLoadStatus = data[0x2B] & 0xff;//地图载入状态 0-成功 1-失败 2-未载入 3-正在载入
                int targetPointId = ByteUtil.bytesToInt(Arrays.copyOfRange(data, 0x2C, 0x30), ByteOrder.LITTLE_ENDIAN);//目标点id
                double forwardSpeed = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x30, 0x38), ByteOrder.LITTLE_ENDIAN);//前进速度
                double turnSpeed = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x38, 0x40), ByteOrder.LITTLE_ENDIAN);//转弯速度
                double batteryVoltage = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x40, 0x48), ByteOrder.LITTLE_ENDIAN);//电池电压，正充电，负放电
                double current = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x48, 0x50), ByteOrder.LITTLE_ENDIAN);//电池电流，正充电，负放电
                int taskStatus = data[0x50] & 0xff;//当前任务状态 0-无前往导航点的任务 1-等待 2-正在前往导航点 3-暂停 4-完成 5-失败 6-退出 7-等待开/关门
                int reserved1 = data[0x51] & 0xff; //保留位1
                int mapVersion = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x52, 0x54), ByteOrder.LITTLE_ENDIAN); //地图版本号
                byte[] reserved2 = Arrays.copyOfRange(data, 0x54, 0x58); // 保留位2
                double totalMileage = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x58, 0x60), ByteOrder.LITTLE_ENDIAN); //累计里程 m
                double currentRunTime = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x60, 0x68), ByteOrder.LITTLE_ENDIAN); //本次运行时间 ms
                double totalRunTime = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x68, 0x70), ByteOrder.LITTLE_ENDIAN);//累计运行时间 ms
                int robotPositionStatus = data[0x70] & 0xff;//定位状态 0-失败 1-成功 2-定位中 3-定位完成
                byte[] reserved3 = Arrays.copyOfRange(data, 0x71, 0x74);//保留位3
                int mapCount = ByteUtil.bytesToInt(Arrays.copyOfRange(data, 0x74, 0x78), ByteOrder.LITTLE_ENDIAN);//地图数量
                String currentMapName = new String(Arrays.copyOfRange(data, 0x78, 0xb8), StandardCharsets.UTF_8).trim();//当前地图名称
                float confidence = ByteUtil.bytesToFloat(Arrays.copyOfRange(data, 0xb8, 0xbc), ByteOrder.LITTLE_ENDIAN);//置信度0~1
                byte[] reserved4 = Arrays.copyOfRange(data, 0xbc, 0xbc + 4);//保留位4
                KcRobotRunningStatus kcRobotRunningStatus = new KcRobotRunningStatus(bodyTemperature, positionX, positionY, orientationAngle, batteryPower, isBlocked,
                        isCharging, runMode, mapLoadStatus, targetPointId, forwardSpeed, turnSpeed, batteryVoltage,
                        current, taskStatus, reserved1, mapVersion, reserved2, totalMileage, currentRunTime,
                        totalRunTime, robotPositionStatus, reserved3, mapCount, currentMapName, confidence, reserved4,1);
                log.debug("[kc]robot running status:{}", kcRobotRunningStatus);
                if (!Objects.equals(lastRunningStatus.getIsCharging(),kcRobotRunningStatus.getIsCharging()) || (int)(lastRunningStatus.getBatteryPower()*100)!=(int)(kcRobotRunningStatus.getBatteryPower()*100)){
                    KcLocationDto kcLocationDto=(KcLocationDto)KcCacheUtil.get(KcCacheUtil.KEY_LOCATION_STATUS);
                    String currentCode=kcLocationDto==null? "":kcLocationDto.getCurrentCode();
                    String distance= kcLocationDto==null? "":kcLocationDto.getDistance();
                    log.info("[kc]current power:{}%,{}V,{}A,charge threshold:{}%,location:{},{}",(int)(kcRobotRunningStatus.getBatteryPower() * 100),df.format(kcRobotRunningStatus.getBatteryVoltage()),df.format(kcRobotRunningStatus.getCurrent()),KcToolkit.robot.getChargeThreshold(),currentCode,distance);
                }
                KcCacheUtil.put(KcCacheUtil.KEY_RUNNING_STATUS, kcRobotRunningStatus);
                if (taskStatus==KcNavStatusEnum.NAV_FINISH.getStatus() && Objects.equals(String.valueOf(targetPointId),KcToolkit.targetPosCode)){
                    //清除目标点记录
                    KcToolkit.targetPosCode = "";
                }
                //每6秒处理一次信息
                if (publishStatusLimiter.tryAcquire(6)) {
                    SpringUtil.getApplicationContext().publishEvent(kcRobotRunningStatus);
                }
                break;
            }
            case 0x1D: {
                //查询机器人导航状态
                KcNavStatus lastNavStatus =Optional.ofNullable((KcNavStatus) KcCacheUtil.get(KcCacheUtil.KEY_NAV_STATUS)).orElse(new KcNavStatus());
                int status = data[0] & 0xff;//导航状态 0-无到导航点任务 1-等待 2-正在前往导航点 3-暂停 4-完成 5失败 6-退出 7-等待开/关门
                byte[] reserved1 = Arrays.copyOfRange(data, 0x01, 0x04);//保留位1
                short targetPointId = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x04, 0x06), ByteOrder.LITTLE_ENDIAN);//目标点id
                byte[] reserved2 = Arrays.copyOfRange(data, 0x06, 0x08);//保留位2
                short[] passedPathPointIds = new short[126];//已经经过的路径点 ID 0 表示无效路径点
                for (int i = 0; i < 126; i++) {
                    passedPathPointIds[i] = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x08 + i * 2, 0x08 + (i + 1) * 2), ByteOrder.LITTLE_ENDIAN);
                }
                short[] unPassedPathPointIds = new short[126];//未经过的路径点 ID 0 表示无效路径点
                for (int i = 0; i < 126; i++) {
                    unPassedPathPointIds[i] = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x104 + i * 2, 0x104 + (i + 1) * 2), ByteOrder.LITTLE_ENDIAN);
                }
                if (!Objects.equals(lastNavStatus.getStatus(), status)){
                    log.info("[kc] robot go to pos:{},{}",targetPointId, KcNavStatusEnum.getDes(status));
                    KcNavStatusEnum navStatusEnum = KcNavStatusEnum.getEnum(status);
                    //非碰撞条触发原因的导航失败（碰撞条触发后台会下发停止队列，状态如果是未完成会变退出）
                    if (Objects.equals(navStatusEnum, KcNavStatusEnum.NAV_FAIL) && StrUtil.isNotBlank(KcToolkit.targetPosCode) ){
                        KcToolkit.publishWarningEvent("导航告警","导航队列失败","", "",1);
                    }
                    // 出现过在狭窄的充电房在差不多充电点的位置，再次发送前往充电点，机器人队列退出，实际已经到了
                    KcLocationDto kcLocationDto=(KcLocationDto)KcCacheUtil.get(KcCacheUtil.KEY_LOCATION_STATUS);
                    if (Objects.equals(navStatusEnum, KcNavStatusEnum.NAV_EXIT)
                            && Objects.equals(KcToolkit.targetPosCode,kcLocationDto.getCurrentCode())
                            && kcLocationDto.getIsReach()
                    ){
                        KcToolkit.targetPosCode = "";
                        log.info("[kc]导航退出，但是位置已经到了");
                    }
                }
                KcNavStatus kcNavStatus = new KcNavStatus(status, reserved1, targetPointId, reserved2, passedPathPointIds, unPassedPathPointIds);
                log.debug("[kc]robot nav status:{}", kcNavStatus);
                KcCacheUtil.put(KcCacheUtil.KEY_NAV_STATUS,kcNavStatus);
                //路径变化发布事件
                if (status==KcNavStatusEnum.NAV_GOING.getStatus() && !Arrays.equals(lastNavStatus.getPassedPathPointIds(),kcNavStatus.getPassedPathPointIds())){
                    SpringUtil.getApplicationContext().publishEvent(kcNavStatus);
                }
                break;
            }
            case 0x1F:
                 //确认初始位置
                 break;
            default:
                log.warn("[kc]unknown cmd:{},seq:{},data:{}", message.getCmdHexStr(), message.getSequence(), data);
                break;
        }
        //释放线程
        KcToolkit.release(message);
    }

    @Override
    public void exceptionCaught(ChannelHandlerContext ctx, Throwable cause) {
        log.error("[kc]handler Exception occurred: ", cause);
        ctx.close();
        KcToolkit.client.reconnect();
        log.warn("[kc]udp client reconnect");
        new Thread(()->{
            try {
                Thread.sleep(5000);
            } catch (InterruptedException e) {
                throw new RuntimeException(e);
            }
            if (StrUtil.isNotBlank(KcToolkit.targetPosCode)){
                KcToolkit.navToPosControl(0, KcToolkit.targetPosCode);
                //可能是导航重启的原因，需要重新发送点位
                log.info("udp重连，重发点位,{}", KcToolkit.targetPosCode);
            }
        }).start();
    }



    private boolean checkExecuteCode(KcMessage dto) {
        byte executeCode = dto.getExecuteCode();
        ExecuteCodeEnum executeCodeEnum = ExecuteCodeEnum.codeMap.get(executeCode);
        if (executeCodeEnum == null) {
            log.warn("[kc]seq:{},cmd:{},unknown executeCode：{}",dto.getSequence(),dto.getCmdHexStr(), executeCode);
            return false;
        }
        switch (executeCodeEnum){
            case SUCCESS -> {
                log.debug("[kc]seq:{},cmd:{},{}", dto.getSequence(),dto.getCmdHexStr(), executeCodeEnum.getDesc());
                if (dto.getCmd()==(byte)0x1D){
                    navFailCount.set(0);
                }
                return true;
            }
            case FAILED -> {
                if (dto.getCmd() == (byte) 0x01) {
                    byte[] data = dto.getData();
                    //x00 U8[16] 变量名，采用 ascii 编码
                    byte[] nameBytes = Arrays.copyOfRange(data, 0x00, 0x10);
                    String varName = new String(nameBytes, StandardCharsets.US_ASCII).trim();
                    log.info("[kc]变量:{},获取失败判断为当前控制器无这个变量,移除后续查询", varName);
                    DataTypeEnum.UN_QUERY_VAR_NAME_SET.add(varName);
                    return false;
                }else if (dto.getCmd() == (byte) 0x1D){
                    //导航查询失败,可以反映出科聪导航程序的状态
                    if (navFailCount.get()<51) {
                        navFailCount.getAndIncrement();
                    }
                }
            }
            case SERVICE_CODE_ERROR,CMD_CODE_ERROR,MESSAGE_HEADER_ERROR -> {
                //无
            }
            default -> KcToolkit.publishWarningEvent("控制器告警",executeCodeEnum.getDesc(), HexUtil.toHex(executeCodeEnum.getCode()), "无",1);
        }
        log.error("[kc]seq:{},cmd:{},desc:{}", dto.getSequence(), dto.getCmdHexStr(), executeCodeEnum.getDesc());
        if (navFailCount.get()==50){
            KcToolkit.publishWarningEvent("控制器告警","导航异常", "", "无",1);
        }
        //todo 发布报警事件
        //publishWarningEvent(HexUtil.toHex(executeCodeEnum.getCode()), executeCodeEnum.getDesc());
        return false;
    }

    private void publishWarningEvent(String executeCode,String content) {
        AddWarningDto addWarningDto = new AddWarningDto();
        addWarningDto.setCode(executeCode);
        addWarningDto.setTitle("控制器报警");
        addWarningDto.setContent(content);
        addWarningDto.setSuggestion("无");
        addWarningDto.setLevel(1);
        SpringUtil.getApplicationContext().publishEvent(addWarningDto);
    }



}
